<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=utf-8">
<title>par_kinematics: /home/wouter/ros_packages/thp-pj2-08/par_kinematics/src/armmodel.cpp Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>

<table border="0">
<tr><td valign="top" style="border-right: 1px solid #333; padding-right: 10px;" >
<p>
Project: <a href="http://ros.org/wiki/par_kinematics">par_kinematics</a>
</p>
<p>
License: BSD
</p>
<p>
Dependencies: 
<ul>
  <li>std_msgs</li>
  <li>roscpp</li>
  <li>par_trajectory_planning</li>
</ul>
</p>
<p>
Used by:
None<br />
</p>
<p>
<a href="../../index.html">All Packages</a>
</p>

</div>
</td><td valign="top" style="padding-left: 10px;" >
<!-- Generated by Doxygen 1.7.1 -->
<div class="navigation" id="top">
  <div class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
    </ul>
  </div>
  <div class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<div class="header">
  <div class="headertitle">
<h1>/home/wouter/ros_packages/thp-pj2-08/par_kinematics/src/armmodel.cpp</h1>  </div>
</div>
<div class="contents">
<a href="armmodel_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00007"></a>00007 <span class="preprocessor">#include &lt;<a class="code" href="armmodel_8h.html">par_kinematics/armmodel.h</a>&gt;</span>
<a name="l00008"></a>00008 
<a name="l00009"></a><a class="code" href="classArmModel.html#a442413d8223d30a804c38784bac0af3a">00009</a> <a class="code" href="classArmModel.html#a442413d8223d30a804c38784bac0af3a">ArmModel::ArmModel</a>(<span class="keywordtype">double</span> angle, <span class="keywordtype">double</span> hip, <span class="keywordtype">double</span> ankle, <span class="keywordtype">double</span> base, <span class="keywordtype">double</span> effector) 
<a name="l00010"></a>00010 {
<a name="l00011"></a>00011     this-&gt;angle = <a class="code" href="classUtil.html#a96a574ae5971d45cc92cbc19677cbe5d" title="Function for converting degrees to radians.">Util::rad</a>(angle);
<a name="l00012"></a>00012     this-&gt;base = base;
<a name="l00013"></a>00013     this-&gt;effector = effector;
<a name="l00014"></a>00014     <a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a> = hip;
<a name="l00015"></a>00015     <a class="code" href="classArmModel.html#aadaa6904d92741aa270a1cf8c3ecb9c4">a</a> = ankle;
<a name="l00016"></a>00016     <a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a> = <span class="keyword">new</span> <a class="code" href="classPoint.html">Point</a>(0,0,0);
<a name="l00017"></a>00017 }
<a name="l00018"></a>00018                 
<a name="l00019"></a><a class="code" href="classArmModel.html#a92773479e178e6f090e77fc19df2cd85">00019</a> <a class="code" href="classArmModel.html#a442413d8223d30a804c38784bac0af3a">ArmModel::ArmModel</a>(<span class="keyword">const</span> <a class="code" href="classArmModel.html">ArmModel</a>&amp; p) 
<a name="l00020"></a>00020 {
<a name="l00021"></a>00021     this-&gt;<a class="code" href="classArmModel.html#a3e469a0741f6e29e6b11950df601c9c0">alpha</a> = <a class="code" href="classArmModel.html#a3e469a0741f6e29e6b11950df601c9c0">alpha</a>;
<a name="l00022"></a>00022     this-&gt;<a class="code" href="classArmModel.html#ae03cad3fb0cdae2de17b71e2e8af6229">beta</a> = <a class="code" href="classArmModel.html#ae03cad3fb0cdae2de17b71e2e8af6229">beta</a>;
<a name="l00023"></a>00023     this-&gt;<a class="code" href="classArmModel.html#a6135ea3cb1411df7dddd40f43c2c6fc5">rho</a> = <a class="code" href="classArmModel.html#a6135ea3cb1411df7dddd40f43c2c6fc5">rho</a>;
<a name="l00024"></a>00024     this-&gt;<a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a> = <a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a>;
<a name="l00025"></a>00025     this-&gt;<a class="code" href="classArmModel.html#a465bd0a8945f7509114240a7af0906c0">base</a> = <a class="code" href="classArmModel.html#a465bd0a8945f7509114240a7af0906c0">base</a>;
<a name="l00026"></a>00026     this-&gt;<a class="code" href="classArmModel.html#aa26d9b7ae909db4dbef4de004f95e4e2">effector</a> = <a class="code" href="classArmModel.html#aa26d9b7ae909db4dbef4de004f95e4e2">effector</a>;
<a name="l00027"></a>00027     this-&gt;<a class="code" href="classArmModel.html#a6901a15b221d275ce26a659bfeee7a8d">angle</a> = <a class="code" href="classArmModel.html#a6901a15b221d275ce26a659bfeee7a8d">angle</a>;
<a name="l00028"></a>00028     this-&gt;<a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a> = <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>;
<a name="l00029"></a>00029     this-&gt;<a class="code" href="classArmModel.html#a62c37ad0350bc177c9c008ac8fcbb875">pangle</a> = <a class="code" href="classArmModel.html#a62c37ad0350bc177c9c008ac8fcbb875">pangle</a>;
<a name="l00030"></a>00030     this-&gt;<a class="code" href="classArmModel.html#a4145e8c786a08caa40b37c926d57fb48">ankleAngle</a> = <a class="code" href="classArmModel.html#a4145e8c786a08caa40b37c926d57fb48">ankleAngle</a>;
<a name="l00031"></a>00031     this-&gt;<a class="code" href="classArmModel.html#aadaa6904d92741aa270a1cf8c3ecb9c4">a</a> = <a class="code" href="classArmModel.html#aadaa6904d92741aa270a1cf8c3ecb9c4">a</a>;
<a name="l00032"></a>00032     this-&gt;<a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a> = <a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a>;
<a name="l00033"></a>00033     this-&gt;<a class="code" href="classArmModel.html#a4d1b74b7b6f8e0ecf56fa56e3fc34aa3">c</a> = <a class="code" href="classArmModel.html#a4d1b74b7b6f8e0ecf56fa56e3fc34aa3">c</a>;
<a name="l00034"></a>00034 }
<a name="l00035"></a>00035                         
<a name="l00036"></a><a class="code" href="classArmModel.html#a8b0321e28d01ac19423d028756b39606">00036</a> <span class="keywordtype">double</span> <a class="code" href="classArmModel.html#a8b0321e28d01ac19423d028756b39606">ArmModel::moveto</a>(<span class="keywordtype">double</span> xg, <span class="keywordtype">double</span> yg, <span class="keywordtype">double</span> zg) 
<a name="l00037"></a>00037 {
<a name="l00038"></a>00038     <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a> = <span class="keyword">new</span> <a class="code" href="classPoint.html">Point</a>(xg, yg, zg);
<a name="l00039"></a>00039     <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#adb0cd22cfdf1ed24b87c77eebe5cff9e">rotateY</a>(-<a class="code" href="classArmModel.html#a6901a15b221d275ce26a659bfeee7a8d">angle</a>);
<a name="l00040"></a>00040     <span class="comment">// offset the goal by effector size</span>
<a name="l00041"></a>00041     <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#a1ea7f1ae0a256ad408953387b85c9da2">offset</a>(<a class="code" href="classPoint.html">Point</a>(<a class="code" href="classArmModel.html#aa26d9b7ae909db4dbef4de004f95e4e2">effector</a>,0,0));
<a name="l00042"></a>00042 
<a name="l00043"></a>00043     <span class="comment">// find projection of a on the z=0 plane, squared</span>
<a name="l00044"></a>00044     <span class="keywordtype">double</span> a2 = <a class="code" href="classArmModel.html#aadaa6904d92741aa270a1cf8c3ecb9c4">a</a>*<a class="code" href="classArmModel.html#aadaa6904d92741aa270a1cf8c3ecb9c4">a</a> - <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#a05ba3b1dfcb19430582ae953cbbfbded">z</a>*<a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#a05ba3b1dfcb19430582ae953cbbfbded">z</a>;  <span class="comment">//sqrt(a*a - g.z*g.z);</span>
<a name="l00045"></a>00045 
<a name="l00046"></a>00046     <span class="comment">// calculate c with respect to base offset</span>
<a name="l00047"></a>00047     <span class="comment">// pure math version (for verification):</span>
<a name="l00048"></a>00048     <span class="comment">// c = sqrt((xg+effector-base)*(xg+effector-base) + yg*yg);</span>
<a name="l00049"></a>00049     <span class="comment">// but since we have g-spot offset by effector, we can use dist() </span>
<a name="l00050"></a>00050     <span class="comment">// to calculate c like this:</span>
<a name="l00051"></a>00051 
<a name="l00052"></a>00052     <a class="code" href="classArmModel.html#a4d1b74b7b6f8e0ecf56fa56e3fc34aa3">c</a> = <a class="code" href="classUtil.html#a8d52322a903b653dc2866a49114c2832" title="Function for calculating distance between 2 3D-points.">Util::dist</a>(<a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#ab99c56589bc8ad5fa5071387110a5bc7">x</a>, <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a>, 0, <a class="code" href="classArmModel.html#a465bd0a8945f7509114240a7af0906c0">base</a>, 0, 0);
<a name="l00053"></a>00053 
<a name="l00054"></a>00054     <a class="code" href="classArmModel.html#a3e469a0741f6e29e6b11950df601c9c0">alpha</a> = acos( (-a2+<a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a>*<a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a>+<a class="code" href="classArmModel.html#a4d1b74b7b6f8e0ecf56fa56e3fc34aa3">c</a>*<a class="code" href="classArmModel.html#a4d1b74b7b6f8e0ecf56fa56e3fc34aa3">c</a>)/(2*<a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a>*c));
<a name="l00055"></a>00055 
<a name="l00056"></a>00056     <a class="code" href="classArmModel.html#ae03cad3fb0cdae2de17b71e2e8af6229">beta</a> = atan2( <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a>, <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#ab99c56589bc8ad5fa5071387110a5bc7">x</a>-<a class="code" href="classArmModel.html#a465bd0a8945f7509114240a7af0906c0">base</a> );
<a name="l00057"></a>00057 
<a name="l00058"></a>00058     <a class="code" href="classArmModel.html#a6135ea3cb1411df7dddd40f43c2c6fc5">rho</a> = <a class="code" href="classArmModel.html#a3e469a0741f6e29e6b11950df601c9c0">alpha</a> - <a class="code" href="classArmModel.html#ae03cad3fb0cdae2de17b71e2e8af6229">beta</a>;
<a name="l00059"></a>00059 
<a name="l00060"></a>00060     <a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a>-&gt;<a class="code" href="classPoint.html#a7cb951c76a0c975fa06c5a6d76745156">moveto</a>(<a class="code" href="classArmModel.html#a465bd0a8945f7509114240a7af0906c0">base</a>+<a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a>*cos(<a class="code" href="classArmModel.html#a6135ea3cb1411df7dddd40f43c2c6fc5">rho</a>), -<a class="code" href="classArmModel.html#a6b8b7868576b967376eb4c99a7f3f8ce">b</a>*sin(<a class="code" href="classArmModel.html#a6135ea3cb1411df7dddd40f43c2c6fc5">rho</a>), 0);
<a name="l00061"></a>00061 
<a name="l00062"></a>00062     <span class="comment">// parallelogram angles</span>
<a name="l00063"></a>00063     <span class="comment">// angle between y-axis and ankle bone in the plane z=0</span>
<a name="l00064"></a>00064     <a class="code" href="classArmModel.html#a4145e8c786a08caa40b37c926d57fb48">ankleAngle</a> = atan2(<a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#ab99c56589bc8ad5fa5071387110a5bc7">x</a>-<a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a>-&gt;<a class="code" href="classPoint.html#ab99c56589bc8ad5fa5071387110a5bc7">x</a>, <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a>-<a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a>-&gt;<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a>);
<a name="l00065"></a>00065 
<a name="l00066"></a>00066     <span class="comment">// create a point for end effector, offset by location AB</span>
<a name="l00067"></a>00067     <a class="code" href="classPoint.html">Point</a> pend = <a class="code" href="classPoint.html">Point</a>(<a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#ab99c56589bc8ad5fa5071387110a5bc7">x</a> - <a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a>-&gt;<a class="code" href="classPoint.html#ab99c56589bc8ad5fa5071387110a5bc7">x</a>, <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a> - <a class="code" href="classArmModel.html#ac03d09b8aa4abf5987275c2093ac8caf">ab</a>-&gt;<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a>, <a class="code" href="classArmModel.html#a79e2dad57d7d2982b159a89961a54535">g</a>-&gt;<a class="code" href="classPoint.html#a05ba3b1dfcb19430582ae953cbbfbded">z</a>);
<a name="l00068"></a>00068     <span class="comment">// rotate its plane by ankleAngle</span>
<a name="l00069"></a>00069     pend.<a class="code" href="classPoint.html#a51a93e16bc9f3334e3383b638d18c004">rotateZ</a>(<a class="code" href="classArmModel.html#a4145e8c786a08caa40b37c926d57fb48">ankleAngle</a>);
<a name="l00070"></a>00070     <span class="comment">// find the parallelogram joint angle</span>
<a name="l00071"></a>00071     <a class="code" href="classArmModel.html#a62c37ad0350bc177c9c008ac8fcbb875">pangle</a> = atan2(pend.<a class="code" href="classPoint.html#afa38be143ae800e6ad69ce8ed4df62d8">y</a>, pend.<a class="code" href="classPoint.html#a05ba3b1dfcb19430582ae953cbbfbded">z</a>);
<a name="l00072"></a>00072 
<a name="l00073"></a>00073     <span class="keywordflow">return</span> <a class="code" href="classArmModel.html#a6135ea3cb1411df7dddd40f43c2c6fc5">rho</a>;
<a name="l00074"></a>00074 }
</pre></div></div>
</div>
</td></tr></table>

<hr size="1"><div style="align: right;">
<a href="http://ros.org/wiki/par_kinematics">par_kinematics</a><br />
Author(s): Wouter van Teijlingen</br />
<small>autogenerated on Mon Jun 13 2011 17:06:55</small>
</div>
</body>
</html>
